#include "maqueen.h"

float kp, ki, kd;
unsigned long lastTime;
float errSum, lastErr;

//获取误差函数
float getError(float sensorValue, float settingValue, int scale){
    float error;
    error = (sensorValue - settingValue) * scale;
    // if(error > 400){
    //     error = 10000;
    // }
    return error;
}

// 循迹传感器数据转化
uint8_t transfer(int L2, int L1, int R1, int R2){
    uint8_t temp = 0;
    int array[4] = {L2,L1,R1,R2};
    for (int i = 0;i < 4; i++){
        temp |= (array[i]<<i);
    }
    return temp;
}

// 获取循迹偏差
float getGrayError(uint8_t grayValue, int scale){
    int R, L;
    float error;
    switch(grayValue)
    {
    case 0x08:
        L=0;R=3;error=(R-L)*scale;break;
    case 0x0C:
        L=0;R=2;error=(R-L)*scale;break;
    case 0x04:
        L=0;R=1;error=(R-L)*scale;break;
    case 0x06:
        L=0;R=0;error=(R-L)*scale;break;
    case 0x02:
        L=1;R=0;error=(R-L)*scale;break;    
    case 0x03:
        L=2;R=0;error=(R-L)*scale;break;
    case 0x01:
        L=3;R=0;error=(R-L)*scale;break;

    default:
        error=10000;break;
    }
    return error;
}

//设置PID参数
void setTunings(float Kp, float Ki, float Kd){
    kp = Kp;
    ki = Ki;
    kd = Kd;
}

//计算PID
float compute(float err){
   unsigned long now = millis();
   double timeChange = (double)(now - lastTime);
  
   errSum += (err * timeChange);
//    errSum += err;
   float dErr = (err - lastErr) / timeChange;
//    float dErr = err - lastErr;
  
   /*Compute PID Output*/
   float Output = kp * err + ki * errSum + kd * dErr;
  
   /*Remember some variables for next time*/
   lastErr = err;
   lastTime = now;
   
   return Output;
}
